//
// Created by ubuntu on 2023/12/5.
//

#include <glog/logging.h>
#include <iostream>
#include <math.h>
#include <Eigen/Geometry>
#include <Eigen/Core>
#include <pwd.h>
#include <zconf.h>
#include <filesystem>

std::string format_string(const char* format, ...) {
    va_list args;
    va_start(args, format);

    // 确定需要的缓冲区大小
    va_list args_copy;
    va_copy(args_copy, args);
    int size = vsnprintf(nullptr, 0, format, args_copy) + 1;
    va_end(args_copy);

    if (size <= 0) {
        va_end(args);
        return "";
    }

    // 动态分配缓冲区
    std::unique_ptr<char[]> buffer(new char[size]);
    vsnprintf(buffer.get(), size, format, args);
    va_end(args);

    return std::string(buffer.get());
}

int main(int argc, char* argv[])
{
    std::string pw_user = getpwuid(getuid())->pw_dir;
    // 设置日志目录
    FLAGS_log_dir = pw_user + "/log";
    // 如果目录不存在，则创建（C++17）
    std::filesystem::create_directories(FLAGS_log_dir);
    // google::InitGoogleLogging("glog_test");// 使用glog之前必须先初始化库，仅需执行一次，括号内为程序名
    google::InitGoogleLogging(argv[0]);
    // Optional: parse command line flags
    gflags::ParseCommandLineFlags(&argc, &argv, true);
    FLAGS_alsologtostderr = true;     // 是否将日志输出到文件和stderr
    FLAGS_colorlogtostderr = true;    // 是否启用不同颜色显示
    FLAGS_max_log_size = 100;  // 100MB
    FLAGS_stop_logging_if_full_disk = true;
    // INFO级别的日志都存放到logs目录下且前缀为INFO_
    google::SetLogDestination(google::GLOG_INFO, (FLAGS_log_dir + "/INFO_").c_str());
    // WARNING级别的日志都存放到logs目录下且前缀为WARNING_
    google::SetLogDestination(google::GLOG_WARNING, (FLAGS_log_dir + "/WARNING_").c_str());
    // ERROR级别的日志都存放到logs目录下且前缀为ERROR_
    google::SetLogDestination(google::GLOG_ERROR, (FLAGS_log_dir + "/ERROR_").c_str());
    // FATAL级别的日志都存放到logs目录下且前缀为FATAL_
    google::SetLogDestination(google::GLOG_FATAL, (FLAGS_log_dir + "/FATAL_").c_str());
    LOG(INFO) << "info";
    LOG(WARNING) << "warning";
    LOG(ERROR) << "error";
//    LOG(INFO) << " " << tan((180 - 110 - 21) * M_PI / 180) * 19.5;
    LOG_EVERY_N(INFO, 10) << tan(1 * M_PI / 180) * 20;
    LOG(INFO) << format_string("User %s logged in %d times", "Alice", 42);
    LOG(INFO) << format_string("Memory usage: %.2f MB", 123.456);
    // 仿照LOG/LOG_EVERY_N/LOG_IF/LOG_IF_EVERY_N/LOG_FIRST_N， 实现LOG_STREAM/LOG_EVERY_N_STREAM/LOG_IF_STREAM/LOG_IF_EVERY_N_STREAM/LOG_FIRST_N_STREAM 支持printf风格接口
//    LOG(INFO) << " " << atan((20 / 5)) * 180 / M_PI;
//    LOG(INFO) << " " << tan(60 * M_PI / 180) * 5;
//    LOG(INFO) << " " << std::sqrt(std::pow(0.436, 2) + std::pow(0.138, 2));
//    LOG(INFO) << " " << std::sqrt(std::pow(0.3385, 2) + std::pow(0.297, 2));
//    LOG(INFO) << " " << std::sqrt(std::pow(0.279, 2) + std::pow(0.3867, 2));
//    LOG(INFO) << " " << std::atan(-1) * 180 / M_PI;
//    LOG(INFO) << " " << std::asin(47 * M_PI / 180);
//    LOG(INFO) << " " << 209.1 / std::sin(47 * M_PI / 180);
//    LOG(INFO) << " " << 300 / std::cos(17 * M_PI / 180);
//    LOG(INFO) << " " << acos(209.1 / std::sin(47 * M_PI / 180) * cos(17 * M_PI / 180) / 300) * 180 / M_PI;
//    LOG(INFO) << " " << 313.7 * sin(24.3 * M_PI / 180);
    /*LOG(FATAL) << "fatal";*/
//    0.0160646 -0.0233779 0.999598 -0.00694755;

    Eigen::Matrix4f camera2ground_eigen_;
    camera2ground_eigen_.setIdentity();
    float a = 0.0160646;
    float b = -0.0233779;
    float c = 0.999598;
    Eigen::Vector3f vec_n(a, b, c);
    vec_n.normalize();
    Eigen::Vector3f Z_axis(0, 0, 0);
    if (vec_n[2] > 0) Z_axis[2] = 1.0;
    else Z_axis[2] = -1.0;
    // 求两个向量间的旋转角angle(点积)
    double tem = Z_axis.dot(vec_n); // 分子
    double tep = sqrt(Z_axis.dot(Z_axis) * vec_n.dot(vec_n)); // 分母
    double angle = acos(tem / tep);
    // acos取值范围[-1,1],若超出范围则越界，输出-1.#IND00
    if (std::isnan(angle)) angle = acos(tep / tem);
    // 求旋转轴（叉积）
    Eigen::Vector3f axis2 = vec_n.cross(Z_axis);
    // 求旋转矩阵
    Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
    transform_2.translation() << 0, 0, 0;
    transform_2.rotate(Eigen::AngleAxisf(angle, axis2.normalized())); // 围绕旋转轴旋转angle弧度。
    LOG(WARNING) << "transform_2 \n" << transform_2.rotation().matrix();
    camera2ground_eigen_.block(0, 0, 3, 3) = transform_2.rotation().matrix();
    LOG(WARNING) << "camera2ground_eigen_ \n" << camera2ground_eigen_;
    google::ShutdownGoogleLogging();  // 当要结束glog时必须关闭库，否则会内存溢出
    return 0;
}
// 代码执行后，会在E:\logs文件夹下生成四个log文件
